function controls = generate_waypoint_trajectory(waypoints, dt, total_time, initial_state)
% 基于固定路径点生成轨迹控制序列
%
% 输入:
%   waypoints - 路径点矩阵 [x1, x2, ...; y1, y2, ...] 或 [x; y] 每列一个点
%   dt - 时间步长
%   total_time - 总时间
%   initial_state - 初始位姿 [x; y; theta]
%
% 输出:
%   controls - 控制序列 [v; omega]

    if nargin < 4
        initial_state = [waypoints(1, 1); waypoints(2, 1); 0];
    end
    
    steps = round(total_time / dt);
    controls = zeros(2, steps);
    
    fprintf('\n=== 固定路径点轨迹生成 ===\n');
    fprintf('路径点数量: %d\n', size(waypoints, 2));
    fprintf('起点: (%.1f, %.1f)\n', waypoints(1, 1), waypoints(2, 1));
    fprintf('终点: (%.1f, %.1f)\n', waypoints(1, end), waypoints(2, end));
    
    % 显示所有路径点
    fprintf('\n路径点序列:\n');
    for i = 1:size(waypoints, 2)
        fprintf('  %d: (%.1f, %.1f)\n', i, waypoints(1, i), waypoints(2, i));
    end
    
    % 计算总路径长度
    total_length = 0;
    for i = 2:size(waypoints, 2)
        segment_length = norm(waypoints(:, i) - waypoints(:, i-1));
        total_length = total_length + segment_length;
        fprintf('  段 %d->%d: %.1f m\n', i-1, i, segment_length);
    end
    fprintf('总路径长度: %.1f m\n', total_length);
    
    % 控制参数
    v_max = 1.2;        % 最大线速度
    v_min = 0.4;        % 最小线速度  
    omega_max = 0.5;    % 最大角速度
    arrival_threshold = 0.8;  % 到达阈值
    
    % 模拟机器人状态
    current_state = initial_state;
    current_waypoint_idx = 1;
    
    % 如果初始位置不是第一个路径点，从第一个路径点开始
    if norm(current_state(1:2) - waypoints(:, 1)) > 0.1
        current_waypoint_idx = 1;
    else
        current_waypoint_idx = 2;
    end
    
    fprintf('\n开始生成控制序列...\n');
    
    for t = 1:steps
        % 如果已经到达所有路径点
        if current_waypoint_idx > size(waypoints, 2)
            % 停止或缓慢旋转观察
            controls(:, t) = [0.3; 0.1];
            continue;
        end
        
        % 当前目标点
        target = waypoints(:, current_waypoint_idx);
        
        % 计算到目标的距离和角度
        dx = target(1) - current_state(1);
        dy = target(2) - current_state(2);
        dist = sqrt(dx^2 + dy^2);
        target_angle = atan2(dy, dx);
        
        % 角度差
        angle_error = wrapToPi(target_angle - current_state(3));
        
        % 如果已到达当前路径点，切换到下一个
        if dist < arrival_threshold
            fprintf('  时刻 %.1fs: 到达路径点 %d (%.1f, %.1f)\n', ...
                t*dt, current_waypoint_idx, target(1), target(2));
            current_waypoint_idx = current_waypoint_idx + 1;
            
            if current_waypoint_idx > size(waypoints, 2)
                controls(:, t) = [0; 0];
                continue;
            end
            
            % 更新目标
            target = waypoints(:, current_waypoint_idx);
            dx = target(1) - current_state(1);
            dy = target(2) - current_state(2);
            dist = sqrt(dx^2 + dy^2);
            target_angle = atan2(dy, dx);
            angle_error = wrapToPi(target_angle - current_state(3));
        end
        
        % 计算控制量
        % 角速度控制 - 比例控制
        omega = max(-omega_max, min(omega_max, 2.0 * angle_error));
        
        % 线速度控制 - 角度误差大时减速
        if abs(angle_error) > pi/3
            % 角度误差很大，几乎停止前进，主要转向
            v = v_min * 0.3;
        elseif abs(angle_error) > pi/6
            % 角度误差较大，减速
            v = v_min;
        else
            % 角度误差小，可以加速
            v = v_min + (v_max - v_min) * (1 - abs(angle_error) / (pi/6));
            
            % 接近目标时减速
            if dist < 3.0
                v = v * (dist / 3.0);
            end
        end
        
        controls(:, t) = [v; omega];
        
        % 更新模拟状态（用于控制生成）
        current_state(1) = current_state(1) + v * cos(current_state(3)) * dt;
        current_state(2) = current_state(2) + v * sin(current_state(3)) * dt;
        current_state(3) = current_state(3) + omega * dt;
        current_state(3) = wrapToPi(current_state(3));
    end
    
    fprintf('轨迹生成完成!\n');
    fprintf('预计运行时间: %.1f s\n', total_time);
    fprintf('最终状态: (%.1f, %.1f, %.2f)\n\n', ...
        current_state(1), current_state(2), current_state(3));
end

